import pexpect
import threading
import re


class BagPlayerProcess(threading.Thread):
    def __init__(self) -> None:
        threading.Thread.__init__(self)
        self.rosbag_file_path = ""
        self.command = ""
        self.is_running = False
        self.text_lock = threading.Lock()
        self.pause_lock = threading.Lock()
        self.bag_lock = threading.Lock()
        self.command_lock = threading.Lock()
        self.pause_is_process = False
        self.bag_is_playing = False
        self.bag_time = 0
        self.bag_cur_time = 0
        self.bag_total_time = 0
        self.rosbag_topics = []
        self.bag_statue = "STOPPED"

    def stop(self):
        self.is_running = False

    def SetStatue(self, bag_statue, bag_time, bag_cur_time, bag_total_time):
        self.text_lock.acquire()
        self.bag_statue = bag_statue
        self.bag_time = bag_time
        self.bag_cur_time = bag_cur_time
        self.bag_total_time = bag_total_time
        self.text_lock.release()

    def GetStatue(self):
        self.text_lock.acquire()
        statue = self.bag_statue
        bag_time = self.bag_time
        bag_cur_time = self.bag_cur_time
        bag_total_time = self.bag_total_time
        self.text_lock.release()
        return statue, bag_time, bag_cur_time, bag_total_time

    def SetBagInfo(self, bag_path, topics: list, rate: str):
        self.rosbag_file_path = bag_path
        self.rosbag_topics = topics
        self.rosbag_rate = rate
        command = f"rosbag play {self.rosbag_file_path} --topics "
        for topic in self.rosbag_topics:
            command += topic + " "
        command += f" --rate {self.rosbag_rate}"
        self.command = command
        return command

    def set_pause(self, pause_is_process):
        self.pause_lock.acquire()
        self.pause_is_process = pause_is_process
        self.pause_lock.release()

    def is_set_pause(self):
        self.pause_lock.acquire()
        pause_is_process = self.pause_is_process
        self.pause_lock.release()
        return pause_is_process

    def run(self):
        if not self.rosbag_file_path:
            return
        self.rosbag_process = pexpect.spawn(self.command)
        pattern = r'\[([A-Z]+)\s*\]\s*Bag Time:\s*([\d.]+).*?Duration:\s*([\d.]+)\s*/\s*([\d.]+)'
        pattern_roscore = r"\[(\d+\.\d+)\]: \[.*\] Failed to contact master at \[([\w\.]+:\d+)\]"
        self.is_running = True
        while self.is_running:
            if self.is_set_pause():
                self.rosbag_process.sendline(" ")
                self.set_pause(False)
            index = self.rosbag_process.expect(
                [pexpect.TIMEOUT, pexpect.EOF, pattern, pattern_roscore])
            if index == 0:
                print("Timeout")
                break
            elif index == 1:
                self.SetStatue("STOPPED", bag_time,
                               bag_total_time, bag_total_time)
                self.is_running = False
                break
            elif index == 2:
                bag_output = self.rosbag_process.after.decode('utf-8')
                matches = re.search(pattern, bag_output)
                if matches:
                    bag_is_playing = matches.group(1)
                    bag_time = matches.group(2)
                    bag_cur_time = matches.group(3)
                    bag_total_time = matches.group(4)
                    self.SetStatue(bag_is_playing, bag_time,
                                   bag_cur_time, bag_total_time)
            elif index == 3:
                matches = re.search(
                    pattern_roscore, self.rosbag_process.after.decode('utf-8'))
                if matches:
                    self.is_running = False
                    self.SetStatue("FAILED_ROSCORE", "0", "0", "0")
                    break
        self.is_running = "STOPPED"
        self.rosbag_process.close()
